function [line1, line2, line3] = cal_lines_six(q, DH_para)

Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;

T01 = TransformationMatrixMDH(Alpha(1), A(1), D(1), Theta(1), q(1));
T12 = TransformationMatrixMDH(Alpha(2), A(2), D(2), Theta(2), q(2));
T23 = TransformationMatrixMDH(Alpha(3), A(3), D(3), Theta(3), q(3));
T34 = TransformationMatrixMDH(Alpha(4), A(4), D(4), Theta(4), q(4));
T45 = TransformationMatrixMDH(Alpha(5), A(5), D(5), Theta(5), q(5));
T56 = TransformationMatrixMDH(Alpha(6), A(6), D(6), Theta(6), q(6));
T02 = T01 * T12;
T03 = T02 * T23;
T04 = T03 * T34;
T05 = T04 * T45;
T06 = T05 * T56;

line1.p1 = T02(1:3, 4);
line1.p2 = T03(1:3, 4);

line2.p1 = T04(1:3, 4) - T04(1:3, 3) * D(4);
line2.p2 = T04(1:3, 4);

line3.p1 = T05(1:3, 4);
line3.p2 = T06(1:3, 4);